package org.riverhillrobotics.frc2013.practice.BrandensVisionCode;

import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.Timer;

public class DriveRobot extends IterativeRobot {
    Joystick leftStick, rightStick, xboxStick;
    RobotDrive robot;
    Jaguar launchM1, launchM2, turretMotor, elevatorLeft, elevatorRight;
    Relay  frisbeeLaunch;
    
    public void robotInit() {
        //Joysticks = contrillers
        leftStick = new Joystick(1); 
        rightStick = new Joystick(2);
        xboxStick = new Joystick(3);
        //Jaguars = variable speed motors
        robot = new RobotDrive(1,2,3,4);
        launchM1 = new Jaguar(5);
        launchM2 = new Jaguar(6);
        turretMotor = new Jaguar(7);
        elevatorLeft = new Jaguar(8);
        elevatorRight = new Jaguar(9);
        //Relays = 1 speed motors
        frisbeeLaunch = new Relay(1);
    }
    
    public void autonomousPeriodic() {
    }

    public void teleopPeriodic() {
        //driving
        robot.tankDrive(leftStick, rightStick); // Joysticks
        while(rightStick.getRawButton(1) == true){ // Left Joystick Tigger
            robot.tankDrive(1.0, 1.0);
        }
        while(leftStick.getRawButton(1) == true){ // Right Joystick Trigger
            robot.tankDrive(-1.0, -1.0);
        }
        
        //x-box controller
        turretMotor.set(Math.abs(xboxStick.getZ())); // Right Stick
        elevatorRight.set(Math.abs(xboxStick.getY())); // Left Stick
        elevatorLeft.set(Math.abs(xboxStick.getTwist())); // Right Stick
        /*while(xboxStick.getRawButton(1) == true){ //x button
        }
        while(xboxStick.getRawButton(2) == true){ // a button
        }
        while(xboxStick.getRawButton(3) == true){ // b button  
        }
        while(xboxStick.getRawButton(4) == true){ // y button
        }*/
        while(xboxStick.getRawButton(5) == true){ // left bumper
            launchM1.set(1.0);
            launchM2.set(1.0);
        }
        while(xboxStick.getRawButton(6) == true){ // right bumper
            frisbeeLaunch.set(Relay.Value.kForward);
        }
        while(xboxStick.getRawButton(7) == true){ // left trigger
            elevatorLeft.set(1.0);
            elevatorRight.set(1.0);
        }
        while(xboxStick.getRawButton(8) == true){ // right trigger
            elevatorLeft.set(1.0);
            elevatorRight.set(1.0);
        }
    }
    
    public void testPeriodic() {
    }  
}